%author: wxj233
%time: 2024.1.11 12:00
%function: 100次蒙特卡洛实验

clc;
clear;
clear Function;
close all;

% 3倍sigma点就是精度
p_d = 0.4/3;  % 测距精度0.4m
p_v = 0.02778/3;  % 测速精度0.0277m/s
p_theta = 0.3/180*pi/3; % 测角精度0.3°（近距离），远距离是0.1°。都是波束中心附近

% 初始化仿真器
% dt
dt = 0.072;



cluster_vars = []; % 记录聚类中心方法跟踪误差方差
cluster_means = []; % 记录聚类中心方法跟踪误差均值
ET_vars = [];  % 记录我的方法的跟踪误差方差
ET_means = [];  % 记录我的方法的跟踪误差均值
for i=1:1:100
    simulater = Simulater(dt);
    disp("第"+num2str(i)+"次计算");
    % 产生第三个目标特征点群
    % len, width, probabilitys, varargin{seed}
    cluster3 = simulater.generateFeaturePoints(5, 2.2, [0.5, 0.5, 0.5,0.5], i);
    % % 根据特征点群，产生第三条轨迹
    % cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
    [T3_1, rT, ep3_1, ev3_1, et3_1] = simulater.generateTrack(cluster3, [-150, 60], [10, 0], [0, 0], 0, [0, 30], 3, p_d, p_v, p_theta, 100+i, 200+i*10);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
    
    % vMax, dr, dvr, dtheta, q, p, vAssDoor, inDoor,initDoor, fplifeMax, tlifeMax, extR, amend
    clusterTracer = ClusterTracer(40, p_d^2, p_v^2, p_theta^2, 0.1, 1, 0.95, 1, 10, 5, 15, 3);
    mfpTracer = MFPTracer(40, p_d^2, p_v^2, p_theta^2, 0.1, 1, 0.95, 0.8, 0.80, 10, 5, 15, 3);
    [frame, isNextFrame]= simulater.getNextFrame();
    
    frames = [];
    while isNextFrame
        frame_c = clusterTracer.preproccess_cluster(frame, 20, 2, 1, 1, 10);  % 预处理points, epsilon, minpts, zoomX, zoomY, zoomV
        frame = mfpTracer.preproccess(frame, 10, 2, 1, 1, 10);  % 预处理points, epsilon, minpts, zoomX, zoomY, zoomV
        frames = [frames; frame];  % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
    
        clusterTracer.tracer(frame_c);
        mfpTracer.tracer(frame);
        [frame, isNextFrame]= simulater.getNextFrame();
    end
    clusterTracer.finish();
    mfpTracer.finish();

    fp = clusterTracer.deadFps(1);
    points = fp.points(:, [1, 13,15]);
    points(:,1) = round(points(:,1), 3);
    
%     figure(i)  % 跟踪差误图
    Ic = ismember(rT(:,1), points(:, 1));
    x = points(:,2)-rT(Ic, 2);
    y = points(:,3)-rT(Ic, 3);
    error_clu = sqrt(x.*x+y.*y);
    cluster_means = [cluster_means, mean(error_clu)];
    cluster_vars = [cluster_vars, var(error_clu)];
%     plot(points(:,1), error_clu, 'DisplayName', "聚类中心方法");hold on;


%     gscatter(frames(:,10), frames(:,11), frames(:,12));hold on; 
    for track = mfpTracer.deadTracks  % [1t, 2x, 3vx, 4y,5vy; ...]
% %         plot(track.points(:,2), track.points(:,4));hold on; 
%         for fp = track.fps
%             plot(fp.points(:,13), fp.points(:,15), 'DisplayName', num2str(track.id));hold on;
%             te = "T"+num2str(fp.track.id)+" f:"+num2str(fp.id);
%             text(fp.points(end,13), fp.points(end,15), te);
%         end
        if size(track.points,1) > 100
            Ic = ismember(rT(:,1), track.points(:,1));
            x = track.points(:,2)-rT(Ic, 2);
            y = track.points(:,4)-rT(Ic, 3);
            error_ET = sqrt(x.*x+y.*y);
            ET_means = [ET_means, mean(error_ET)];
            ET_vars = [ET_vars, var(error_ET)];
%             plot(track.points(:,1), error_ET,'DisplayName', "ET-SC");hold on;
        end
    end

%     title("跟踪误差("+num2str(i)+")");
% %     axis([-160 160 0 100]);
%     xlabel("t(s)");
%     ylabel("误差(m)");
%     legend;
%     hold off;
%     disp("pause");
end


figure(3)
plot(1:1:100, cluster_means,'DisplayName', "聚类中心方法");hold on;
plot(1:1:100, ET_means,'DisplayName', "ET-SC");hold on;
title("跟踪误差均值");
xlabel("实验序号");
ylabel("误差均值(m)");
legend;
hold off;


figure(4)
plot(1:1:100, cluster_vars,'DisplayName', "聚类中心方法");hold on;
plot(1:1:100, ET_vars,'DisplayName', "ET-SC");hold on;
title("跟踪误差方差");
xlabel("实验序号");
ylabel("误差方差");
legend;

